#include "Arduino.h"
#include "PIcontrol.h"

/*

PIcontrol contains classes to implement single variable and
3-vector integrators and Proportional-Integral (PI) controls

class PIinteg
        
        PIinteg(void);
        PIinteg(int K_i, int ex2);
        PIinteg(int K_i, int ex2, int init)
        
        int update(int input, int dt);
        int getValue(void);
        void setGains(int K_a, int ex2);
        void setIntegrator(int init);
      
      
      PIintegLimit : PIinteg
      
        PIintegLimit(int K_i, int ex2, int limit)
      
        void setGains(int K_a, int ex2, int limit);
        void setLimit(int limit);
        void setLimit(int minLim, int maxLim);
      
      
      PIcontrol : PIinteg
      
        PIcontrol(void);
        PIcontrol(int K_p, int K_a, int ex2);
        PIcontrol(int K_p, int K_a, int ex2, int init);
        
        int update(int input, int dt);
        void setGains(int K_p, int K_a, int ex2);
        
        
      PIlowGain
      
      
      PIcontrolLimit
      PIlowGainLimit
      
      
                               
                               
                               
                               
                               
      PIVinteg
      LongVinteg
      PIVcontrol
      PIVlowGain
      
      
      PIVintegLimit
      LongVintegLimit
      PIVcontrolLimit
      PIVlowGainLimit
      
      
                               
      FFFilter

*/


//**********************************************************
// PIinteg - Simple Integrator
//**********************************************************

PIinteg::PIinteg(void) {
  setIntegrator(0);
}

PIinteg::PIinteg(int K_i, int ex2) {
  setGains(K_i, ex2);
  setIntegrator(0);
}

PIinteg::PIinteg(int K_i, int ex2, int init) {
  setGains(K_i, ex2);
  setIntegrator(init);
}


int PIinteg::update(int input, int dt) {
  long x = long(input) * k_a * dt;
  integrator += x;
  return int(integrator >> kShift);
}

void PIinteg::setIntegrator(int init) {
  integrator = long(init) * 1000;
}

int PIinteg::getValue(void) {
  return int(integrator >> kShift);
}

void PIinteg::setGains(int K_i, int ex2) {
  k_a = int((long(K_i) << 10) / 1000);
  kShift = ex2 + 10;
}

//**********************************************************
// PIintegLimit - Simple Limited Integrator
//**********************************************************

PIintegLimit::PIintegLimit(void) {
  setIntegrator(0);
  setLimit(0x7FFF);
  
}

PIintegLimit::PIintegLimit(int K_i, int ex2) {
  setGains(K_i, ex2, 0x7FFF);
  setIntegrator(0);
}

PIintegLimit::PIintegLimit(int K_i, int ex2, int limit) {
  setGains(K_i, ex2, limit);
}

int PIintegLimit::update(int input, int dt) {
  long x = long(input) * k_a * dt;
  addInt(x);
  return int(integrator >> kShift);
}

void PIintegLimit::addInt(long x) {
  long headHi = maxLimit - integrator;
  long headLo = minLimit - integrator;
  if ((x < headHi) && (x > headLo)) integrator += x;
}

void PIintegLimit::setGains(int K_i, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_a = int((long(K_i) << 10) / 1000);
  kShift = ex2 + 10;
  setLimit(maxL, minL);
}

void PIintegLimit::setGains(int K_i, int ex2, int limit) {
  setGains(K_i, ex2);
  setLimit(limit);
}

void PIintegLimit::setLimit(int limit) {
  maxLimit = long(limit) << kShift;
  minLimit = -maxLimit;
}

void PIintegLimit::setLimit(int maxLim, int minLim) {
  maxLimit = long(maxLim) << kShift;
  minLimit = long(minLim) << kShift;
}

//**********************************************************
// PIVinteg - 3-Vector Integrator
//**********************************************************

PIVinteg::PIVinteg(void) {
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVinteg::PIVinteg(int K_i, int ex2) {
  setGains(K_i, ex2);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVinteg::PIVinteg(int K_i, int ex2, int init[]) {
  setGains(K_i, ex2);
  setIntegrator(init);
}

void PIVinteg::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]) * k_a * dt;
    integrator[i] += x;
    output[i] = int(integrator[i] >> kShift);
  }
}

void PIVinteg::setIntegrator(int init[]) {
  for (int i = 0; i < 3; i++) {
    integrator[i] = long(init[i]) * 1000;
  }
}

void PIVinteg::getValue(int output[]) {
  for (int i = 0; i < 3; i++) {
    output[i] = int(integrator[i] >> kShift);
  }
}

void PIVinteg::setGains(int K_i, int ex2) {
  k_a = int((long(K_i) << 10) / 1000);
  kShift = ex2 + 10;
}


//**********************************************************
// PIVintegLimit - Simple Limited Integrator
//**********************************************************

PIVintegLimit::PIVintegLimit(void) {
  setLimit(0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVintegLimit::PIVintegLimit(int K_i, int ex2) {
  setGains(K_i, ex2, 0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVintegLimit::PIVintegLimit(int K_i, int ex2, int limit) {
  setGains(K_i, ex2, limit);
}

void PIVintegLimit::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]) * k_a * dt;
    addInt(x, i);
    output[i] = int(integrator[i] >> kShift);
  }
}

void PIVintegLimit::addInt(long x, int idx) {
  long headHi = maxLimit - integrator[idx];
  long headLo = minLimit - integrator[idx];
  if ((x < headHi) && (x > headLo)) integrator[idx] += x;
}

void PIVintegLimit::setGains(int K_i, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_a = int((long(K_i) << 10) / 1000);
  kShift = ex2 + 10;
  setLimit(maxL, minL);
}

void PIVintegLimit::setGains(int K_i, int ex2, int limit) {
  setGains(K_i, ex2);
  setLimit(limit);
}

void PIVintegLimit::setLimit(int limit) {
  maxLimit = long(limit) << kShift;
  minLimit = -maxLimit;
}

void PIVintegLimit::setLimit(int maxLim, int minLim) {
  maxLimit = long(maxLim) << kShift;
  minLimit = long(minLim) << kShift;
}

//**********************************************************
// LongVinteg - 3-Vector 48 bit Integrator
//**********************************************************

LongVinteg::LongVinteg(void) {
  int i[] = {0,0,0};
  setIntegrator(i);
}

LongVinteg::LongVinteg(int K_i, int ex2) {
  setGains(K_i, ex2);
  int i[] = {0,0,0};
  setIntegrator(i);
}

LongVinteg::LongVinteg(int K_i, int ex2, int init[]) {
  setGains(K_i, ex2);
  setIntegrator(init);
}

void LongVinteg::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]);
    integrator_low[i] += x * k_a * dt;
    if (integrator_low[i] > 0xFFFFL) {
      integrator[i] += integrator_low[i] >> 16;
      integrator_low[i] &= 0xFFFFL;
    } else if (-integrator_low[i] > 0x10000L) {
      integrator[i] += integrator_low[i] >> 16;
      integrator_low[i] |= 0xFFFF0000L;
    }
    output[i] = int(integrator[i] >> kShift);
  }
}

void LongVinteg::setIntegrator(int init[]) {
  for (int i = 0; i < 3; i++) {
    integrator[i] = long(init[i]) * 1000;
    integrator_low[i] = 0;
  }
}


void LongVinteg::setGains(int K_i, int ex2) {
  k_a = int((long(K_i) << 10) / 1000);
  kShift = ex2 - 6;
}


//**********************************************************
// LongVintegLimit - 3-Vector 48 bit Limited Integrator
//**********************************************************

LongVintegLimit::LongVintegLimit(void) {
  setLimit(0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

LongVintegLimit::LongVintegLimit(int K_i, int ex2) {
  setGains(K_i, ex2, 0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

LongVintegLimit::LongVintegLimit(int K_i, int ex2, int init[]) {
  setGains(K_i, ex2);
  setIntegrator(init);
}

void LongVintegLimit::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]);
    integrator_low[i] += x * k_a * dt;
    if (integrator_low[i] > 0xFFFFL) {
      x = integrator_low[i] >> 16;
      integrator_low[i] &= 0xFFFFL;
    } else if (-integrator_low[i] > 0x10000L) {
      x = integrator_low[i] >> 16;
      integrator_low[i] |= 0xFFFF0000L;
    }
    addInt(x, i);
    output[i] = int(integrator[i] >> kShift);
  }
}


//**********************************************************
// PIcontrol - PI control
//**********************************************************

PIcontrol::PIcontrol(void) {
  integrator = 0;
}

PIcontrol::PIcontrol(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2);
  integrator = 0;
}

PIcontrol::PIcontrol(int K_p, int K_a, int ex2, int init) {
  setGains(K_p, K_a, ex2);
  setIntegrator(init);
}

int PIcontrol::update(int input, int dt) {
  long x = long(input) * k_a;
  integrator += x * dt;
  x = (x * k_p) + integrator + pFrac;
  pFrac = x & fracMask;
  return int(x >> kShift);
}

void PIcontrol::setGains(int K_p, int K_a, int ex2) {
  k_p = K_p;
  k_a = int((long(K_a) << 10) / 1000);
  kShift = ex2 + 10;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
}

//**********************************************************
// PIcontrolLimit - PI control
//**********************************************************

PIcontrolLimit::PIcontrolLimit(void) {
  setLimit(0x7FFF);
  integrator = 0;
}

PIcontrolLimit::PIcontrolLimit(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2, 0x7FFF);
  integrator = 0;
}

PIcontrolLimit::PIcontrolLimit(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2, limit);
  integrator = 0;
}

int PIcontrolLimit::update(int input, int dt) {
  long x = long(input)* k_a;
  long xx = x * dt;
  addInt(xx);
  x = (x * k_p) + integrator + pFrac;
  pFrac = x & fracMask;
  return int(x >> kShift);
}

void PIcontrolLimit::setGains(int K_p, int K_a, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_p = K_p;
  k_a = int((long(K_a) << 10) / 1000);
  kShift = ex2 + 10;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
  setLimit(maxL, minL);
}

void PIcontrolLimit::setGains(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2);
  setLimit(limit);
}


//**********************************************************
// PIVcontrol - 3-Vector PI control
//**********************************************************


PIVcontrol::PIVcontrol(void) {
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVcontrol::PIVcontrol(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVcontrol::PIVcontrol(int K_p, int K_a, int ex2, int init[]) {
  setGains(K_p, K_a, ex2);
  setIntegrator(init);
}

void PIVcontrol::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]) * k_a;
    integrator[i] += x * dt;
    x = (x * k_p) + integrator[i] + pFrac[i];
    pFrac[i] = x & fracMask;
    output[i] = int(x >> kShift);
  }
}

void PIVcontrol::setGains(int K_p, int K_a, int ex2) {
  k_p = K_p;
  k_a = int((long(K_a) << 10) / 1000);
  kShift = ex2 + 10;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
}


//**********************************************************
// PIVcontrolLimit - 3-Vector PI control
//**********************************************************


PIVcontrolLimit::PIVcontrolLimit(void) {
  setLimit(0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVcontrolLimit::PIVcontrolLimit(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2, 0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVcontrolLimit::PIVcontrolLimit(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2, limit);
  int i[] = {0,0,0};
  setIntegrator(i);
}

void PIVcontrolLimit::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]) * k_a;
    long xx = x * dt;
    addInt(xx, i);
    x = (x * k_p) + integrator[i] + pFrac[i];
    pFrac[i] = x & fracMask;
    output[i] = int(x >> kShift);
  }
}
void PIVcontrolLimit::setGains(int K_p, int K_a, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_p = K_p;
  k_a = int((long(K_a) << 10) / 1000);
  kShift = ex2 + 10;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
  setLimit(maxL, minL);
}

void PIVcontrolLimit::setGains(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2);
  setLimit(limit);
}


//**********************************************************
// PIlowGain - PI control with low gain
//**********************************************************


PIlowGain::PIlowGain(void) {
  integrator = 0;
  integrator_low = 0;
}

PIlowGain::PIlowGain(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2);
  integrator_low = 0;
  integrator = 0;
}

PIlowGain::PIlowGain(int K_p, int K_a, int ex2, int init) {
  setGains(K_p, K_a, ex2);
  integrator_low = 0;
  setIntegrator(init);
}

int PIlowGain::update(int input, int dt) {
	long x = long(input);
	integrator_low += x * k_a * dt;
	if (integrator_low > 0xFFFFL) {
		integrator += integrator_low >> 16;
		integrator_low &= 0xFFFFL;
	} else if (-integrator_low > 0x10000L) {
		integrator += integrator_low >> 16;
		integrator_low |= 0xFFFF0000L;
	}
	x *= k_p;
	x += integrator + pFrac;
  pFrac = x & fracMask;
	return int(x >> kShift);
}

void PIlowGain::setIntegrator(int init) {
  integrator = long(init) * 1000;
  integrator_low = 0;
  pFrac = 0;
}

void PIlowGain::setGains(int K_p, int K_a, int ex2) {
  k_a = int((long(K_a) << 10) / 1000);
  k_p = int(((long(K_p) * long(k_a))/10) >> 6);
  kShift = ex2 - 6;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
}


//**********************************************************
// PIlowGainLimit - PI control with low gain
//**********************************************************


PIlowGainLimit::PIlowGainLimit(void) {
  setLimit(0x7FFF);
  integrator = 0;
  integrator_low = 0;
}

PIlowGainLimit::PIlowGainLimit(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2, 0x7FFF);
  integrator_low = 0;
  integrator = 0;
}

PIlowGainLimit::PIlowGainLimit(int K_p, int K_a, int ex2, int init) {
  setGains(K_p, K_a, ex2);
  integrator_low = 0;
  setIntegrator(init);
}

int PIlowGainLimit::update(int input, int dt) {
	long x = long(input);
	integrator_low += x * k_a * dt;
	if (integrator_low > 0xFFFFL) {
		addInt(integrator_low >> 16);
		integrator_low &= 0xFFFFL;
	} else if (-integrator_low > 0x10000L) {
		addInt(integrator_low >> 16);
		integrator_low |= 0xFFFF0000L;
	}
	x *= k_p;
	x += integrator + pFrac;
  pFrac = x & fracMask;
	return int(x >> kShift);
}

void PIlowGainLimit::setIntegrator(int init) {
  integrator = long(init) * 1000;
  integrator_low = 0;
  pFrac = 0;
}

void PIlowGainLimit::setGains(int K_p, int K_a, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_a = int((long(K_a) << 10) / 1000);
  k_p = int(((long(K_p) * long(k_a))/10) >> 6);
  kShift = ex2 - 6;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
  setLimit(maxL, minL);
}

void PIlowGainLimit::setGains(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2);
  setLimit(limit);
}


//**********************************************************
// PIVlowGain - 3-Vector PI control with low gain
//**********************************************************


PIVlowGain::PIVlowGain(void) {
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVlowGain::PIVlowGain(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVlowGain::PIVlowGain(int K_p, int K_a, int ex2, int init[]) {
  setGains(K_p, K_a, ex2);
  setIntegrator(init);
}

void PIVlowGain::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]);
    integrator_low[i] += x * k_a * dt;
    if (integrator_low[i] > 0xFFFFL) {
      integrator[i] += integrator_low[i] >> 16;
      integrator_low[i] &= 0xFFFFL;
    } else if (-integrator_low[i] > 0x10000L) {
      integrator[i] += integrator_low[i] >> 16;
      integrator_low[i] |= 0xFFFF0000L;
    }
    x *= k_p;
	x += integrator[i] + pFrac[i];
    pFrac[i] = x & fracMask;
    output[i] = int(x >> kShift);
  }
}

void PIVlowGain::setIntegrator(int init[]) {
  for (int i = 0; i < 3; i++) {
    integrator[i] = long(init[i]) * 1000;
    integrator_low[i] = 0;
    pFrac[i] = 0;
  }
}

void PIVlowGain::setGains(int K_p, int K_a, int ex2) {
  k_a = int((long(K_a) << 10) / 1000);
  k_p = int(((long(K_p) * long(k_a))/10) >> 6);
  kShift = ex2 - 6;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
}


//**********************************************************
// PIVlowGainLimit - 3-Vector PI control with low gain
//**********************************************************


PIVlowGainLimit::PIVlowGainLimit(void) {
  setLimit(0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVlowGainLimit::PIVlowGainLimit(int K_p, int K_a, int ex2) {
  setGains(K_p, K_a, ex2, 0x7FFF);
  int i[] = {0,0,0};
  setIntegrator(i);
}

PIVlowGainLimit::PIVlowGainLimit(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2, limit);
  int i[] = {0,0,0};
  setIntegrator(i);
}

void PIVlowGainLimit::update(int input[], int output[], int dt) {
  for (int i = 0; i < 3; i++) {
    long x = long(input[i]);
    integrator_low[i] += x * k_a * dt;
    if (integrator_low[i] > 0xFFFFL) {
      addInt(integrator_low[i] >> 16, i);
      integrator_low[i] &= 0xFFFFL;
    } else if (-integrator_low[i] > 0x10000L) {
      addInt(integrator_low[i] >> 16, i);
      integrator_low[i] |= 0xFFFF0000L;
    }
    x *= k_p;
    x += integrator[i] + pFrac[i];
    pFrac[i] = x & fracMask;
    output[i] = int(x >> kShift);
  }
}

void PIVlowGainLimit::setIntegrator(int init[]) {
  for (int i = 0; i < 3; i++) {
    integrator[i] = long(init[i]) * 1000;
    integrator_low[i] = 0;
    pFrac[i] = 0;
  }
}

void PIVlowGainLimit::setGains(int K_p, int K_a, int ex2) {
  int maxL = int(maxLimit >> kShift);
  int minL = int(minLimit >> kShift);
  k_a = int((long(K_a) << 10) / 1000);
  k_p = int(((long(K_p) * long(k_a))/10) >> 6);
  kShift = ex2 - 6;
  fracMask = 0;
  for (int i = 0; i < kShift; i++) {
    fracMask = (fracMask << 1) + 1;
  }
  setLimit(maxL, minL);
}

void PIVlowGainLimit::setGains(int K_p, int K_a, int ex2, int limit) {
  setGains(K_p, K_a, ex2);
  setLimit(limit);
}

//**********************************************************
// FFFilter - Feed-Forward Filter
//**********************************************************


FFFilter::FFFilter(void)
{
  integ2 = 0;
}

FFFilter::FFFilter(int K_p, int K_a, int ex2) : 
  PIcontrol(K_p, K_a, ex2)
{
  integ2 = 0;
}

int FFFilter::update(int input, int dt) {
  return update(input, 0, dt);
}

int FFFilter::update(int input, int feedForward, int dt) {
  long x = long(input) - (integ2 >> 10);
  x *= k_a;
  integrator += dt * x;
  x = (x * k_p) + integrator + pFrac;
  pFrac = x & fracMask;
  vOut = int(x >> kShift);
  integ2 += dt * (vOut + feedForward);
  return int(integ2 >> 10);
}

void FFFilter::setIntegrator2(long init) {
  integ2 = init;
}

//**********************************************************
// LongFFFilter - Feed-Forward Filter with Long Input/Output
//**********************************************************


LongFFFilter::LongFFFilter(void)
{
  integ2 = 0;
  outInt = 0;
}

LongFFFilter::LongFFFilter(int K_p, int K_a, int ex2) : 
  FFFilter(K_p, K_a, ex2)
{
  integ2 = 0;
  outInt = 0;
}

long LongFFFilter::update(long input, int dt) {
  return update(input, 0, dt);
}

long LongFFFilter::update(long input, int feedForward, int dt) {
  long x = input - outInt;
  x *= k_a;
  integrator += dt * x;
  x = (x * k_p) + integrator + pFrac;
  pFrac = x & fracMask;
  vOut = int(x >> kShift);
  integ2 += dt * (vOut + feedForward);
  long temp = integ2 >> 10;
  outInt += temp;
  integ2 -= temp << 10;
  return outInt;
}

